#!/usr/bin/env python
PACKAGE = "path_tracking_pid"

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t
from math import sqrt

gen = ParameterGenerator()

gen.add("l", double_t, 0, "Distance between axle and control point", 0.5, -10, 10)
gen.add("target_x_vel", double_t, 0, "Target forward velocity", 2.0, -10, 10)
gen.add("target_end_x_vel", double_t, 0, "Target forward velocity", 0.0, -10, 10)
gen.add("target_x_acc", double_t, 0, "Target forward acceleration", 2.0, 1e-9, 10)
gen.add("target_x_decc", double_t, 0, "Target forward decceleration", 2.0, 1e-9, 10)
gen.add("abs_minimum_x_vel", double_t, 0, "Absolute minimum velocity for reaching target", 0.025, 0.0, 0.5)
gen.add("max_error_x_vel", double_t, 0, "Maximum allowed error in forward velocity", 1.0, 1e-9, 10)
gen.add("max_x_vel", double_t, 0, "Maximum x velocity, used for scaling purposes", 5.0, 0, 10)
gen.add("max_yaw_vel", double_t, 0, "Maximum yaw velocity", 2.0, 0, 10)
gen.add("max_yaw_acc", double_t, 0, "Maximum yaw acceleration", 2.0, 0, 10)
gen.add("min_turning_radius", double_t, 0, "Minimum turning radius", 0.0, 0.0, 100)
gen.add("track_base_link", bool_t, 0, "Should the controller track the path using the base_link frame?",  False)

gen.add("init_vel_method", int_t, 0, "Initial velocity method", 1, 0, 3, edit_method=gen.enum([
    gen.const("Zero",             int_t, 0, "Always start from zero"),
    gen.const("InternalSetpoint", int_t, 1, "Last internal setpoint is new initial setpoint"),
    gen.const("Odom",             int_t, 2, "Start from current odometry value")
    ], "Initial velocity method enum"))
gen.add("init_vel_max_diff", double_t, 0, "Maximum vel-diff allowed when starting a path (-1 to ignore, only active upon 'init_vel_method'==InternalSetpoint)", 0.5, -1, 10)

gen.add("Kp_lat", double_t, 0, "Kp Lateral", 1, 0, 10)
gen.add("Ki_lat", double_t, 0, "Ki Lateral", 0, 0, 2)
gen.add("Kd_lat", double_t, 0, "Kd Lateral", 0.3, 0, 10)

gen.add("Kp_ang", double_t, 0, "Kp Angular", 1, 0, 10)
gen.add("Ki_ang", double_t, 0, "Ki Angular", 0, 0, 2)
gen.add("Kd_ang", double_t, 0, "Kd Angular", 0.3, 0, 10)

gen.add("lowpass_cutoff", double_t, 0, "Lowpass cutoff (Hz), 0 disables the filter", 0, 0, 1000)
gen.add("lowpass_damping", double_t, 0, "Lowpass damping", sqrt(2), 0, 10)

gen.add("feedback_lat",   bool_t,   0, "Enable lateral feedback?",  True)
gen.add("feedback_ang",   bool_t,   0, "Enable angular feedback?",  False)

gen.add("feedforward_lat",   bool_t,   0, "Enable lateral feedforward?",  False)
gen.add("feedforward_ang",   bool_t,   0, "Enable angular feedforward?",  False)

gen.add("controller_debug_enabled",   bool_t,   0, "Debug controller intermediate gains",  False)

gen.add("use_mpc", bool_t, 0, "Limit forward velocity based on predictions of the lateral error", False)
grp_mpc = gen.add_group("mpc_group", type="hide")
grp_mpc.add("mpc_simulation_sample_time", double_t, 0, "MPC simulation sample time", 0.05, 1e-9, 10)
grp_mpc.add("mpc_max_error_lat", double_t, 0, "MPC maximum allowed lateral error", 0.5, 1e-9, 10)
grp_mpc.add("mpc_max_fwd_iterations", int_t, 0, "MPC maximum allowed iterations forward in time", 200, 0, 1000000)
grp_mpc.add("mpc_min_x_vel", double_t, 0, "MPC minimum absolute forward velocity", 0.5, 1e-9, 10)
grp_mpc.add("mpc_max_vel_optimization_iterations", int_t, 0, "MPC maximum allowed velocity optimization iterations", 5, 1, 1000)

grp_tricycle = gen.add_group("Tricycle")
grp_tricycle.add("max_steering_angle", double_t, 0, "Maximum steering angle for tricycle model", 3.1416, 0, 3.1416)
grp_tricycle.add("max_steering_x_vel", double_t, 0, "Maximum steering x velocity for tricycle model", 3.0, 0, 10)
grp_tricycle.add("max_steering_x_acc", double_t, 0, "Maximum steering x acceleration for tricycle model", 2.0, 0, 10)
grp_tricycle.add("max_steering_yaw_vel", double_t, 0, "Maximum steering yaw velocity for tricycle model", 0.5, 0, 10)
grp_tricycle.add("max_steering_yaw_acc", double_t, 0, "Maximum steering yaw acceleration for tricycle model", 0.5, 0, 10)

gen.add("anti_collision", bool_t, 0, "Stop on lethal obstacles", False)
collision_group = gen.add_group("collision_group", type="hide")
collision_group.add("obstacle_speed_reduction", bool_t, 0, "Slow down on near obstacles", True)
collision_group.add("collision_look_ahead_length_offset", double_t, 0, "Offset in length to project rectangle collision along path", 1.0, 0, 5)
collision_group.add("collision_look_ahead_resolution", double_t, 0, "Spatial resolution to project rectangle collision along path", 1.0, 1e-9, 10)

exit(gen.generate(PACKAGE, "path_tracking_pid", "Pid"))
